﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Design;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Microsoft.Ccr.Core;

namespace HighLevelControl
{

    public partial class UI : Form
    {
        public UI()
        {
            InitializeComponent();
        }

        public string UpdateDataR1(PioneerControl.Proxy.Replace robot1)
        {
            groupBox1.BackColor = Color.GreenYellow;
            #region Robot 1
            if (robot1 != null)
            {
                ClearBumpers1();

                ActiveBehavior.Text = robot1.Body.ActiveBehavior.ToString();
                Task.Text = robot1.Body.Task.ToString();

                #region LRFTop
                //Sets the Top View
                int width1 = 630;
                int height1 = 300;
                Bitmap bmp1 = new Bitmap(width1, height1);
                Graphics g1 = Graphics.FromImage(bmp1);
                g1.Clear(Color.Blue);
                double angularOffset = -90 + robot1.Body.LaserState.AngularRange / 2.0;
                double piBy180 = Math.PI / 180.0;
                double halfAngle = robot1.Body.LaserState.AngularResolution / 2.0;
                double scale = height1 / 5000.0;

                System.Drawing.Drawing2D.GraphicsPath path = new System.Drawing.Drawing2D.GraphicsPath();
                for (int pass = 0; pass != 2; pass++)
                {
                    for (int i = 0; i < robot1.Body.LaserState.DistanceMeasurements.Length; i++)
                    {
                        int range = robot1.Body.LaserState.DistanceMeasurements[i];
                        if (range > 0 && range < 8192)
                        {
                            double angle = i * robot1.Body.LaserState.AngularResolution - angularOffset;
                            double lowAngle = (angle - halfAngle) * piBy180;
                            double highAngle = (angle + halfAngle) * piBy180;

                            double drange = range * scale;

                            float lx = (float)(height1 + drange * Math.Cos(lowAngle));
                            float ly = (float)(height1 - drange * Math.Sin(lowAngle));
                            float hx = (float)(height1 + drange * Math.Cos(highAngle));
                            float hy = (float)(height1 - drange * Math.Sin(highAngle));

                            if (pass == 0)
                            {
                                if (i == 0)
                                {
                                    path.AddLine(height1, height1, lx, ly);
                                }
                                path.AddLine(lx, ly, hx, hy);
                            }
                            else
                            {
                                g1.DrawLine(Pens.DarkBlue, lx, ly, hx, hy);
                            }
                        }
                    }

                    if (pass == 0)
                    {
                        g1.FillPath(Brushes.White, path);
                    }
                }

                float botWidth = (float)(190 * scale);
                g1.DrawLine(Pens.Red, height1, height1 - botWidth, height1, height1);
                g1.DrawLine(Pens.Red, height1 - 3, height1 - botWidth, height1 + 3, height1 - botWidth);
                g1.DrawLine(Pens.Red, height1 - botWidth, height1 - 3, height1 - botWidth, height1);
                g1.DrawLine(Pens.Red, height1 + botWidth, height1 - 3, height1 + botWidth, height1);
                g1.DrawLine(Pens.Red, height1 - botWidth, height1 - 1, height1 + botWidth, height1 - 1);
                g1.DrawString(robot1.Body.MostRecentLaser.ToString(), new Font(FontFamily.GenericSansSerif, 40, GraphicsUnit.Pixel), Brushes.Gray, 0, 0);
                g1.DrawString(robot1.Body.Heading.ToString(), new Font(FontFamily.GenericSansSerif, 40, GraphicsUnit.Pixel), Brushes.Gray, 0, 50);
                LRFTop.Image = bmp1;

                #endregion

                #region Camera
                if (robot1.Body.WebcamData != null)
                    cam1.Image = MakeBitmap(robot1.Body.WebcamData.Size.Width, robot1.Body.WebcamData.Size.Height, robot1.Body.WebcamData.Frame);
                #endregion

                #region Bumpers

                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper0)
                    rectangleShape1.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper1)
                    rectangleShape2.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper2)
                    rectangleShape3.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper3)
                    rectangleShape4.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper4)
                    rectangleShape5.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper5)
                    rectangleShape6.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper6)
                    rectangleShape7.FillColor = Color.Red;

                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper0)
                    rectangleShape8.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper1)
                    rectangleShape9.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper2)
                    rectangleShape10.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper3)
                    rectangleShape11.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper4)
                    rectangleShape12.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper5)
                    rectangleShape13.FillColor = Color.Red;
                if (robot1.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper6)
                    rectangleShape14.FillColor = Color.Red;

                #endregion

                #region Commands
                if (comboBox1.Text != null)
                    return comboBox1.Text;
                else
                    return "";
                #endregion
            }
            else
                return "";
            #endregion
        }

        public string UpdateDataR2(PioneerControl.Proxy.Replace robot2)
        {
            groupBox2.BackColor = Color.GreenYellow;
            #region Robot 2
            if (robot2 != null)
            {
                ClearBumpers2();

                ActiveBehavior2.Text = robot2.Body.ActiveBehavior.ToString();
                Task.Text = robot2.Body.Task.ToString();

                #region LRFTop
                //Sets the Top View
                int width2 = 630;
                int height2 = 300;
                double piBy180 = Math.PI / 180.0;
                Bitmap bmp2 = new Bitmap(width2, height2);
                Graphics g2 = Graphics.FromImage(bmp2);
                g2.Clear(Color.Blue);
                double angularOffset2 = -90 + robot2.Body.LaserState.AngularRange / 2.0;
                double halfAngle2 = robot2.Body.LaserState.AngularResolution / 2.0;
                double scale2 = height2 / 5000.0;

                System.Drawing.Drawing2D.GraphicsPath path2 = new System.Drawing.Drawing2D.GraphicsPath();
                for (int pass = 0; pass != 2; pass++)
                {
                    for (int i = 0; i < robot2.Body.LaserState.DistanceMeasurements.Length; i++)
                    {
                        int range = robot2.Body.LaserState.DistanceMeasurements[i];
                        if (range > 0 && range < 8192)
                        {
                            double angle = i * robot2.Body.LaserState.AngularResolution - angularOffset2;
                            double lowAngle = (angle - halfAngle2) * piBy180;
                            double highAngle = (angle + halfAngle2) * piBy180;

                            double drange = range * scale2;

                            float lx = (float)(height2 + drange * Math.Cos(lowAngle));
                            float ly = (float)(height2 - drange * Math.Sin(lowAngle));
                            float hx = (float)(height2 + drange * Math.Cos(highAngle));
                            float hy = (float)(height2 - drange * Math.Sin(highAngle));

                            if (pass == 0)
                            {
                                if (i == 0)
                                {
                                    path2.AddLine(height2, height2, lx, ly);
                                }
                                path2.AddLine(lx, ly, hx, hy);
                            }
                            else
                            {
                                g2.DrawLine(Pens.DarkBlue, lx, ly, hx, hy);
                            }
                        }
                    }

                    if (pass == 0)
                    {
                        g2.FillPath(Brushes.White, path2);
                    }
                }

                float botWidth2 = (float)(190 * scale2);
                g2.DrawLine(Pens.Red, height2, height2 - botWidth2, height2, height2);
                g2.DrawLine(Pens.Red, height2 - 3, height2 - botWidth2, height2 + 3, height2 - botWidth2);
                g2.DrawLine(Pens.Red, height2 - botWidth2, height2 - 3, height2 - botWidth2, height2);
                g2.DrawLine(Pens.Red, height2 + botWidth2, height2 - 3, height2 + botWidth2, height2);
                g2.DrawLine(Pens.Red, height2 - botWidth2, height2 - 1, height2 + botWidth2, height2 - 1);
                g2.DrawString(robot2.Body.MostRecentLaser.ToString(), new Font(FontFamily.GenericSansSerif, 40, GraphicsUnit.Pixel), Brushes.Gray, 0, 0);
                g2.DrawString(robot2.Body.Heading.ToString(), new Font(FontFamily.GenericSansSerif, 40, GraphicsUnit.Pixel), Brushes.Gray, 0, 50);
                LRFTop2.Image = bmp2;

                #endregion

                #region Camera
                if (robot2.Body.WebcamData != null)
                    cam2.Image = MakeBitmap(robot2.Body.WebcamData.Size.Width, robot2.Body.WebcamData.Size.Height, robot2.Body.WebcamData.Frame);
                #endregion

                #region Bumpers

                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper0)
                    rectangleShape1b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper1)
                    rectangleShape2b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper2)
                    rectangleShape3b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper3)
                    rectangleShape4b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper4)
                    rectangleShape5b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper5)
                    rectangleShape6b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.FirstBumper6)
                    rectangleShape7b.FillColor = Color.Red;

                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper0)
                    rectangleShape8b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper1)
                    rectangleShape9b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper2)
                    rectangleShape10b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper3)
                    rectangleShape11b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper4)
                    rectangleShape12b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper5)
                    rectangleShape13b.FillColor = Color.Red;
                if (robot2.Body.StallBumpers == Microsoft.Robotics.Services.MobileRobots.Arcos.Proxy.StallAndBumpers.SecondBumper6)
                    rectangleShape14b.FillColor = Color.Red;

                #endregion

                #region Commands
                if (comboBox2.Text != null)
                    return comboBox2.Text;
                else
                    return "";
                #endregion
            }
            else
                return "";
            #endregion
        }


        public void ClearBumpers1()
        {
            rectangleShape1.FillColor = Color.LightGray;
            rectangleShape2.FillColor = Color.LightGray;
            rectangleShape3.FillColor = Color.LightGray;
            rectangleShape4.FillColor = Color.LightGray;
            rectangleShape5.FillColor = Color.LightGray;
            rectangleShape6.FillColor = Color.LightGray;
            rectangleShape7.FillColor = Color.LightGray;
            rectangleShape8.FillColor = Color.LightGray;
            rectangleShape9.FillColor = Color.LightGray;
            rectangleShape10.FillColor = Color.LightGray;
            rectangleShape11.FillColor = Color.LightGray;
            rectangleShape12.FillColor = Color.LightGray;
            rectangleShape13.FillColor = Color.LightGray;
            rectangleShape14.FillColor = Color.LightGray;
        }
        public void ClearBumpers2()
        {

            rectangleShape1b.FillColor = Color.LightGray;
            rectangleShape2b.FillColor = Color.LightGray;
            rectangleShape3b.FillColor = Color.LightGray;
            rectangleShape4b.FillColor = Color.LightGray;
            rectangleShape5b.FillColor = Color.LightGray;
            rectangleShape6b.FillColor = Color.LightGray;
            rectangleShape7b.FillColor = Color.LightGray;
            rectangleShape8b.FillColor = Color.LightGray;
            rectangleShape9b.FillColor = Color.LightGray;
            rectangleShape10b.FillColor = Color.LightGray;
            rectangleShape11b.FillColor = Color.LightGray;
            rectangleShape12b.FillColor = Color.LightGray;
            rectangleShape13b.FillColor = Color.LightGray;
            rectangleShape14b.FillColor = Color.LightGray;
        }
        ///<Creates a Bitmap from the webcam raw RGB data>
        System.Drawing.Bitmap MakeBitmap(int width, int height, byte[] imageData)
        {
            // NOTE: This code implicitly assumes that the width is a multiple
            // of four bytes because Bitmaps have to be longword aligned.
            // We really should look at bmp.Stride to see if there is any padding.
            // However, the width and height come from the webcam and most cameras
            // have resolutions that are multiples of four.
            System.Drawing.Bitmap bmp = new System.Drawing.Bitmap(width, height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
            System.Drawing.Imaging.BitmapData data = bmp.LockBits(
            new System.Drawing.Rectangle(0, 0, bmp.Width, bmp.Height),
            System.Drawing.Imaging.ImageLockMode.WriteOnly,
            System.Drawing.Imaging.PixelFormat.Format24bppRgb
            );
            System.Runtime.InteropServices.Marshal.Copy(imageData, 0, data.Scan0, imageData.Length);
            bmp.UnlockBits(data);
            return bmp;
        }

    }
}
